#include "nh_base_alg.h"





void InsertionSort_float(float *data, int size)
{
    int i = 1;
    int j;
    float  v;
    while (i < size) 
    {
        v = data[i];
        j = i - 1;

        while (j >= 0 && v < data[j]) {
        data[j + 1] = data[j];
        j--;
        }

        data[j + 1] = v;
        i++;
    }
}

void InsertionSort_uint16(uint16_t *data, int size)
{
    int i = 1;
    int j;
    uint16_t  v;
    while (i < size) 
    {
        v = data[i];
        j = i - 1;

        while (j >= 0 && v < data[j]) {
        data[j + 1] = data[j];
        j--;
        }
        data[j + 1] = v;
        i++;
    }
}



// 插入排序函数实现
void InsertionSortuint32(int32_t *data, uint32_t size)
{
    uint32_t i = 1;
    int32_t j;
    int32_t  v;
    // printf("%s\tline %d\n",__FILE__,__LINE__);
    while (i < size) 
    {
        v = data[i];
        j = i - 1;
        while (j >= 0 && v < data[j]) {
        data[j + 1] = data[j];
        j--;
        }
        data[j + 1] = v;
        i++;
    }
    // printf("%s\tline %d\n",__FILE__,__LINE__);
}

// argsort函数实现
void argsort(int32_t *data, int32_t *shadow_cache,uint32_t size, int32_t *res_arg) {
    // 复制原始数据到临时数组，以便进行排序
    for (uint32_t i = 0; i < size; i++) {
        shadow_cache[i] = data[i];
    }
    // 对临时数组进行排序
    // printf("%s\tline %d\n",__FILE__,__LINE__);
    InsertionSortuint32(shadow_cache, size);
    // printf("%s\tline %d\n",__FILE__,__LINE__);
    // 创建结果数组，用于存储排序后的原始索引
    for (uint32_t i = 0; i < size; i++) {
        for (uint32_t j = 0; j < size; j++) {
            if (shadow_cache[i] == data[j]) {
                res_arg[i] = j;
                break;
            }
        }
    }
}

int32_t listmax(int32_t *data, const uint32_t size) {
    int32_t max = data[0];
    for (uint32_t i = 1; i < size; i++) {
        if (data[i] > max) {
            max = data[i];
        }
    }
    return max;
}

int32_t listmin(int32_t *data, const uint32_t size) {
    int32_t min = data[0];
    for (uint32_t i = 1; i < size; i++) {
        if (data[i] < min) {
            min = data[i];
        }
    }
    return min;
}



uint16_t listmax_uint16( uint16_t *data,const uint32_t size)
{    
    uint16_t max = data[0];
    for (uint32_t i = 1; i < size; i++) {
        if (data[i] > max) {
            max = data[i];
        }
    }
    return max;
}

uint16_t listmin_uint16( uint16_t *data,const uint32_t size)
{
    uint16_t min = data[0];
    for (uint32_t i = 1; i < size; i++) {
    if (data[i] < min) {
    min = data[i];
    }
    }
    return min;
}






const static float para_list[3]={ 0.1765684016f , -0.8651729334f , 1.5214332873f };
const static float Gain = 4.785512021e+01;

float filter_hr_spo2(float input,float *xbuf,float *ybuf)
{
    xbuf[0] = xbuf[1]; xbuf[1] = xbuf[2]; xbuf[2] = xbuf[3];
    xbuf[3] = input / Gain;
    ybuf[0] = ybuf[1]; ybuf[1] = ybuf[2]; ybuf[2] = ybuf[3]; 
    ybuf[3] =   (xbuf[0] + xbuf[3]) + 3 * (xbuf[1] + xbuf[2])
                    + (  para_list[0] * ybuf[0]) + ( para_list[1] * ybuf[1])
                    + (  para_list[2] * ybuf[2]);
    return ybuf[3];
}



/**
 * @brief Finds the maximum value in an array of floats
 *
 * @param data Pointer to the array of floats
 * @param size The number of elements in the array
 * @return float The maximum value found in the array
 */
float find_fmax(float *data, int32_t size) {
    if (data == NULL || size <= 0) {
        return -1.0f; // Or some error value indicating the function failed to find a max value
    }
    
    float max_value = 0.0f; // Initialize to the smallest float value
    for (int32_t i = 0; i < size; ++i) {
        if (data[i] > max_value) {
            max_value = data[i];
        }
    }
    return max_value;
}

float find_fmin(float *data, int32_t size) {
    if (data == NULL || size <= 0) {
        return -1.0f; // Or some error value indicating the function failed to find a min value
    }
    
    float min_value = 1000.0f; // Initialize to the largest float value
    for (int32_t i = 0; i < size; ++i) {
        if (data[i] < min_value) {
            min_value = data[i];
        }
    }
    return min_value;
}





int32_t fitval (int32_t *x_val,int32_t *y_val,int16_t _len,int32_t x_in )
{
    int32_t retval=0;
/// 数据检查需求
/**
 * 1、检查x_val输入的数据中有没有 相同数据
 * 2、检查y_val输入的数据中有没有 0 
 * **/ // 重心法你拟合多项式

    uint8_t ichk,ichk_sub; /// 检查需要的
    for(ichk=0; ichk<_len; ichk++) 
    {   
        for(ichk_sub=0 ; ichk_sub<_len ; ichk_sub++) 
        {   
            if(x_val[ichk] == x_val[ichk_sub]) 
            {
                if(ichk != ichk_sub) 
                {
                     return 0; /// 出现了错误的输入值
                }
            }
        }
        if(y_val[ichk]==0)
        {
            return 0;
        }
    }
/********************/		
		int32_t x_v[3]={0.0};
		int32_t y_v[3]={0.0};
		int32_t x_get=0.0;
		x_get = x_in;
		for(ichk=0;ichk<3;ichk++)
		{
			x_v[ichk] = x_val[ichk];
			y_v[ichk] = y_val[ichk];
		}
		retval=0.0;
/*******************/		
		/** 拟合与计算 **/
    retval=   y_v [0]*((x_get-x_v [1]) *(x_get-x_v [2] )) / ((x_v [0]-x_v [1] ) * (x_v [0]-x_v [2] )) ;
    retval+=  y_v [1]*((x_get-x_v [0]) *(x_get-x_v [2] )) / ((x_v [1]-x_v [0] ) * (x_v [1]-x_v [2] )) ; 
    retval+=  y_v [2]*((x_get-x_v [0]) *(x_get-x_v [1] )) / ((x_v [2]-x_v [0] ) * (x_v [2]-x_v [1] ));
		retval = retval+0.5;
		// NX_PRINTF("fit dep %.3f\r\n",retval);
    return retval;
}



void compute_divided_differences(float x[4], float y[4], float table[4], int n) 
{
    for (int j = 0; j < n; j++) {
        table[j] = y[j];
    }

    for (int i = 1; i < n; i++) {
        for (int j = n - 1; j >= i; j--) {
            table[j] = (table[j] - table[j - 1]) / (x[j] - x[j - i]);
        }
    }
}

// 牛顿重心多项式插值
float newton_divide_diff_interpolate(float x[4], float divided_diff[4], float x_val, int n) 
{
    float result = divided_diff[n - 1];
    for (int i = n - 2; i >= 0; i--) {
        result = divided_diff[i] + (x_val - x[i]) * result;
    }
    return result;
/********* example ***********/
/*
    float xx[4] = {4,34,66,94}; // 已知的x值
    float yx[4] = {80000,86000,84000,81000}; // 已知的y值
    float x_val = 42; // 要插值的x值
    float divided_diff[4]; // 存储差分的数组
    // 计算差分
    compute_divided_differences(xx, yx, divided_diff, 4);
    // 进行插值
    float y_val;
    for(uint16_t ix=0;ix<95;ix++)
    {
        y_val = newton_divide_diff_interpolate(xx, divided_diff, (float)ix, 4);
        printf("%d , %d\n",ix,(int32_t)(y_val+0.5));
    }
*/
/*****************************/
}


/**
 * @brief 
 * 
 * @param data 
 * @param size 
 * @return int32_t 
 */


uint16_t mean_value_uint16(uint16_t *data, int32_t size) 
{
    // 检查参数data是否为NULL
    if (data == NULL) {
        return 0; // 如果data为NULL，返回0作为安全措施
    }
    // 检查参数size是否非负
    if (size < 0) {
        return 0; // 如果size为负数，返回0作为安全措施
    }
    // 使用int64_t来避免可能的整型溢出问题
    int32_t sum = 0;
    for (int32_t i = 0; i < size; ++i) {
        sum += data[i];
    }
    // 计算平均值，确保类型兼容性，四舍五入
    return (uint16_t)((float)sum /(float)size + 0.5f);
}

float mean_value_float(float *data,int32_t size)
{
     if (data == NULL) {
        return 0; // 如果data为NULL，返回0作为安全措施
    }
    if (size < 0) {
        return 0; // 如果size为负数，返回0作为安全措施
    }
        // 使用int64_t来避免可能的整型溢出问题
    float sum = 0.0f;
    for (int32_t i = 0; i < size; ++i) {
        sum += data[i];
    }
    // 计算平均值，确保类型兼容性，四舍五入
    return (float)sum /(float)size;
}


uint16_t mean_value_uint16_Weighted(uint16_t *data,int32_t size)
{
    float graivt = 0.0f,xindex=0.0f;
    float graivt_sum=0.0f;
    float graivt_sum2=0.0f;
    const float use_2pi = 2.0f*3.141593f; 
    for(uint32_t ix=0;ix<size;ix++)
    {
       xindex =  (ix+1.0f)*0.1f*use_2pi/(float)size;
       graivt = xindex - xindex*xindex*xindex/6.0f+xindex*xindex*xindex*xindex*xindex/120.0f; //// sin(xindex)
       graivt_sum += graivt;
    //    printf("pre [%d]-> %f \n",ix,graivt);
    }
    // printf("graivt_sum: %f\n",graivt_sum);
    graivt_sum2 = 0.0f;
    for(uint32_t ix=0;ix<size;ix++)
    {   
       xindex =  (ix+1.0f)*0.1f*use_2pi/(float)size;
       graivt = (xindex - xindex*xindex*xindex/6.0f+xindex*xindex*xindex*xindex*xindex/120.0f)/graivt_sum;  //// sin(xindex)
        // printf("end val: [%3d]->[%f]\n",ix,graivt);
       graivt_sum2 += (float)data[ix]*graivt;
    }
    // printf("graivt_sum: %f\n",graivt_sum2);
    return (uint16_t)(graivt_sum2+0.5f);
}



int32_t time_valid_check(uint16_t *list_a,uint16_t len_a)
{
    int32_t all_len;
    all_len = len_a;
    uint16_t rag =0,valid_cnt=0;;
    uint16_t valid_cnt_thrsh;
    valid_cnt_thrsh = (int16_t)floorf((float)(len_a-1)*0.6f);
    if(valid_cnt_thrsh == 0)
    {
        valid_cnt_thrsh+=1;
    }
    valid_cnt =0;
    uint16_t i,j;
    for (i = 0; i < len_a; i++)
    {
        rag = (uint16_t)ceilf((float)list_a[i]*0.15f);
        valid_cnt=0;
        for(j=0;j<len_a;j++ )
        {
            if(i!=j)
            {
                if( (list_a[j])<=(list_a[i]+rag) && (list_a[j])>=(list_a[i]-rag)  ) /// 在上下限内
                {
                    valid_cnt++;
                }
            }
        }
        if(valid_cnt<valid_cnt_thrsh)
        {
            list_a[i]=0;
            all_len--;
        }
        // printf("_check:[%d] range:[%d]\tvalid_cnt_thrsh:[%d]\tvalid_cnt:[%d]\n",i,rag,valid_cnt_thrsh,valid_cnt);
    } 
    return all_len;
}

#define diff_lim_debug (0x00)
uint16_t diff_lim(uint16_t *res, uint32_t *tick_info ,uint16_t _len,const float diff_lim_s ,float sample_freq)
{
    uint16_t end_loc_val = res[_len-1];
    uint16_t max_lim = 0;
    uint16_t min_lim = 0xffff;
    uint16_t temp_val;
    int16_t tick_diff=0;
#if diff_lim_debug
    //  show raw info
    printf(" res: ");
    for (uint16_t i = 0; i < _len; i++)
    {
        printf("%3d   \t\t",res[i]);
    }
    printf("\n");
    printf("tick: ");
    for (uint16_t i = 0; i < _len; i++)
    {
        printf("%3d   \t\t",tick_info[i]);
    }
    printf("\n[diff,max,min,end]\n");


    printf("temp: ");
    printf("\t\t\t");
#endif
    for(uint16_t i=0;i<_len-1;i++)
    {
        tick_diff = tick_info[i]-tick_info[i+1];
       temp_val = (uint16_t)roundf((float)tick_diff /(float)(sample_freq)*diff_lim_s)+res[i];
#if diff_lim_debug     
       printf("[%2d,%d,",tick_diff,temp_val);
#endif
    //    if(    temp_val > max_lim )
    //    {
            max_lim = temp_val;
    //    }
       temp_val =  res[i] - (uint16_t)roundf((float)(tick_info[i]-tick_info[i+1])/(float)(sample_freq)*diff_lim_s) ;
#if diff_lim_debug
       printf("%d,",temp_val);
#endif
    //    if(    temp_val < min_lim )
    //    {
            min_lim = temp_val;
    //    }
       if(i)
       {
            if(res[i+1]>max_lim)
            {
                res[i+1] = max_lim;
            }
            if(res[i+1]<min_lim)
            {
                res[i+1] = min_lim;
            }
       }
#if diff_lim_debug
        printf("%d]\t",res[i+1]);
#endif   
    }
#if diff_lim_debug
    printf("\n");
    printf("max_lim <%d> min_lim <%d>\n",max_lim,min_lim);
#endif    
    if(end_loc_val > max_lim)
    {
        end_loc_val = max_lim;
    }
    if (end_loc_val < min_lim)
    {
        end_loc_val = min_lim;
    }
#if diff_lim_debug
    printf ("--> end_loc_val <%d>\n",end_loc_val);
#endif
    return end_loc_val;
}


float diff_lim_ratio_dynamic(uint16_t curret_hr)
{
    float end_ratio=2.0f;
    // end_ratio = -0.0081f*(float)curret_hr+3.825f;
    // if(curret_hr<40)
    // {
    //     end_ratio = 3.5f;
    // }
    // if(curret_hr>200)
    // {
    //     end_ratio = 2.2f;
    // }


    end_ratio = 0.0094f*(float)curret_hr+1.625f;
    if(curret_hr<40) {
        end_ratio = 2.0f;
    }
    if(curret_hr>200) {
        end_ratio = 3.5f;
    }
    return end_ratio;
}


